/*
The surface-physics project: http://code.google.com/p/surface-physics/
Adds real world physics based on the Bullet physics engine, to bases in the Orbiter space
flight simulation software.

Written by Abhijit Nandy 
License : Apache License 2.0

Bump_SimpleRigidBody.cpp
Bump_SimpleRigidBody management implementation.


*/


/* Class declaration */
#include "Bump_SimpleRigidBody.h"

Registrar Bump_SimpleRigidBody::registrar("SimpleRigidBody", Bump_SimpleRigidBody::create);

int Bump_SimpleRigidBody::count = 0;

/**
 * Constructor for this derived class
 * Will be used if some component specific variable needs to be initialized.
 * Do NOT allocate memory here, let init() do that
 *
 */
Bump_SimpleRigidBody::Bump_SimpleRigidBody():
		ground(NULL)
{
	rposCurrent = _V(0., 0., 0.);
	angle = 0.;
	
	rx = 0.;
	ry = 0.;
	rz = 0.;

	//Do not set the base for any object here, its still null

}


/**
 * Destructor, does nothing currently
 */
Bump_SimpleRigidBody::~Bump_SimpleRigidBody()
{
	//Destroy physics counterpart
	int rv = destroyBulletRigidBody();
	if(rv){
		sprintf(boutput, "Bump_SimpleRigidBody::~Bump_SimpleRigidBody: ERROR : Destroying rigid body failed for %s", strObjectName);
		oapiWriteLog(boutput);
	}

	count--;
}


/**
 * Initialize a base component.
 * Creates a new BulletBase Mesh and inserts it in the Orbiter world
 * Also creates the corresponding object in the Bullet World.
 *
 */
int
Bump_SimpleRigidBody::clbkSetClassCaps(void)
{
	int rv;


	sprintf(boutput, "Bump_SimpleRigidBody::init: Initializing %s", strObjectName);
	oapiWriteLog(boutput);

	//Create Orbiter object: Load any meshes by reading the mesh files from the physics file
	rv = createOrbiterPrimitives();
	if(rv){
		sprintf(boutput, "Bump_SimpleRigidBody::init: ERROR : Creating Orbiter primitives failed %s",
				strObjectName);
		oapiWriteLog(boutput);
		return 1;
	}

	//Choose the proper terrain generator as picked by the base.
	switch(bumpBase->terrainGenerator){
	case TT_PLANETCURVE:
		ground = new Bump_TerrainPatch(4, 4, 0.000002f, 0.000002f, 0.000003f, 0.000003f, 10.f, bumpBase);
		break;
	case TT_ORULEX:
		ground = new Bump_TerrainPatchOru(4, 4, 0.000002f, 0.000002f, 0.000003f, 0.000003f, 10.f, bumpBase);
		break;
	case TT_D3D11:
		break;
	default:
		//No terrain!!, physics controlled objects will fall forever!
		break;
	}

	if(!ground){
		oapiWriteLogV("Bump_4WheeledVehicle::init: ERROR : Could not add ground for %s", strObjectName);
		return 2;
	}


	//bumpBase->bb->Local2Global(rposCurrent, gposCurrent);
	double lng, lat, rad;
	//oapiLocalToEqu (bumpBase->hObjBulletBase, rposCurrent, &lng, &lat, &rad);

	bumpBase->bb->Local2Global(rposCurrent, gposCurrent);
	oapiGlobalToEqu(bumpBase->hObjRefBody, gposCurrent, &lng, &lat, &rad);

	rv = ground->createPatch(lng, lat);
	if(rv){
		oapiWriteLogV("Bump_SimpleRigidBody::init: ERROR : Could not add ground for %s : %d", strObjectName, rv);
		return 3;
	}


	//Create physics counterpart, if this fails, should remove Orbiter meshes that were loaded above
	rv = createBulletRigidBody();
	if(rv){

		destroyOrbiterPrimitives();

		sprintf(boutput, "Bump_SimpleRigidBody::init: ERROR : Creating rigid body failed %s",
				strObjectName);
		oapiWriteLog(boutput);
		return 1;
	}



	//Only after Orbiter AND Bullet successful, set physics to true
	physicsInited = true;

	count++;

	return 0;
}


/**
 * Updates a base mesh
 * Primarily move the mesh into position according to latest transform by setting
 * translation, yaw ,pitch & roll using animation handles created in init().
 *
 */
int
Bump_SimpleRigidBody::clbkPostStep(double SimT, double SimDT, double MJD)
{


	//sprintf(boutput, "Bump_SimpleRigidBody::update: The next sim step will advance the sim by %f secs", SimDT);
	//oapiWriteLog(boutput);

/*	if(physicsInited){

		//Get the physics transform
		btTransform trans;
		rbSimple->getMotionState()->getWorldTransform(trans);
		btVector3 v = trans.getOrigin();


		VISHANDLE hvisCar = *oapiObjectVisualPtr (oapiGetObjectByName("Brighton Beach"));

		if(hvisCar != NULL){
			MESHGROUP_TRANSFORM mt;

			//-txyz
			mt.P.transparam.shift = -rposCurrent;
			mt.transform = mt.TRANSLATE;
			mt.ngrp = 0;
			mt.nmesh = mshIndex;
			bumpBase->bb->MeshgroupTransform(hvisCar, mt);


			//-ry
			mt.P.rotparam.angle = -ry ;
			mt.P.rotparam.ref =  _V(0.f, 0.f, 0.f);
			mt.P.rotparam.axis = _V(0, 1, 0);
			mt.transform = mt.ROTATE;
			mt.ngrp = 0;
			mt.nmesh = mshIndex;
			bumpBase->bb->MeshgroupTransform(hvisCar, mt);

			//-rx
			mt.P.rotparam.angle = -rx ;
			mt.P.rotparam.ref =  _V(0.f, 0.f, 0.f);
			mt.P.rotparam.axis = _V(1, 0, 0);
			mt.transform = mt.ROTATE;
			mt.ngrp = 0;
			mt.nmesh = mshIndex;
			bumpBase->bb->MeshgroupTransform(hvisCar, mt);

			//-rz
			mt.P.rotparam.angle = -rz ;
			mt.P.rotparam.ref =  _V(0.f, 0.f, 0.f);
			mt.P.rotparam.axis = _V(0, 0, 1);
			mt.transform = mt.ROTATE;
			mt.ngrp = 0;
			mt.nmesh = mshIndex;
			bumpBase->bb->MeshgroupTransform(hvisCar, mt);


			//--------------------------------------------------------------------

			btMatrix3x3 m_el = trans.getBasis();
			ry = btAtan2( m_el[0].z(), m_el[0].x() );
			if(ry < 0)
				ry += SIMD_PI;

			float yaw, pitch, roll;
			trans.getBasis().getEulerYPR(yaw, pitch, roll);
;
			rz = yaw;
			if(rz < 0)
				rz += SIMD_PI;

			rx = roll;
			if(rx < 0)
				rx += SIMD_PI;

			//rz
			mt.P.rotparam.angle = rz ;
			mt.P.rotparam.ref =  _V(0.f, 0.f, 0.f);
			mt.P.rotparam.axis = _V(0, 0, 1);
			mt.transform = mt.ROTATE;
			mt.ngrp = 0;
			mt.nmesh = mshIndex;
			bumpBase->bb->MeshgroupTransform(hvisCar, mt);

			//rx
			mt.P.rotparam.angle = rx ;
			mt.P.rotparam.ref =  _V(0.f, 0.f, 0.f);
			mt.P.rotparam.axis = _V(1, 0, 0);
			mt.transform = mt.ROTATE;
			mt.ngrp = 0;
			mt.nmesh = mshIndex;
			bumpBase->bb->MeshgroupTransform(hvisCar, mt);

			//ry
			mt.P.rotparam.angle = ry ;
			mt.P.rotparam.ref =  _V(0.f, 0.f, 0.f);
			mt.P.rotparam.axis = _V(0, 1, 0);
			mt.transform = mt.ROTATE;
			mt.ngrp = 0;
			mt.nmesh = mshIndex;
			bumpBase->bb->MeshgroupTransform(hvisCar, mt);


			//txyz
			rposCurrent = _V(BT3ARGS(v));
			mt.P.transparam.shift = rposCurrent;
			mt.transform = mt.TRANSLATE;
			mt.ngrp = 0;
			mt.nmesh = mshIndex;
			bumpBase->bb->MeshgroupTransform(hvisCar, mt);


			oapiWriteLogV("%3.5f | (%3.5f, %3.5f, %3.5f) +  (%3.5f, %3.5f, %3.5f)| (%3.5f, %3.5f, %3.5f)",
					angle*DEG, rx*DEG, ry*DEG, rz*DEG, V3ARGS(rposCurrent), yaw*DEG, pitch*DEG, roll*DEG);
			/*sprintf(oapiDebugString(), "%3.5f | %3.5f | (%3.5f, %3.5f, %3.5f) | %3.5f",
					y2*DEG, y1*DEG, yaw*DEG, pitch*DEG, roll*DEG, angle*DEG);*/

/*
		}





		// Apply a rotation
		//trans.setIdentity();
		//trans.setOrigin(v);
		angle += 5.f * RAD;

		if(angle > PI2)
			angle = 0.;

		btQuaternion q;
		q.setEuler( 0., 0., angle);
		trans.setRotation(q);*/

		//rbSimple->getMotionState()->setWorldTransform(trans);
//		rbSimple->setActivationState(DISABLE_DEACTIVATION);  //so bullet gets transform from body
		

	double state = 0.0f;



	//sprintf(boutput, "Bump_Plate::update: The next sim step will advance the sim by %f secs", SimDT);
	//oapiWriteLog(boutput);

	if(physicsInited){

		//Get the physics transform
		btTransform transComponent;
		btVector3 originComponent;

		rbSimple->getMotionState()->getWorldTransform(transComponent);
		//trans.getOpenGLMatrix(m);
		originComponent = transComponent.getOrigin();
		x = originComponent.x();
		y = originComponent.y();
		z = originComponent.z();
		transComponent.getBasis().getEulerYPR(rz, ry, rx);

		//sprintf(output,"Sphere height: %f, rx=%f, ry=%f, rz=%f\n", trans.getOrigin().getY(), rx*180/PI, ry*180/PI, rz*180/PI);
		//oapiWriteLog(output);


		//Update the Orbiter mesh
		state = fabs(x/Bump_BulletBase::BULLET_WORLD_SIZE);
		state *= 0.5; //reduce range to within  0 to 0.5 or 0.5 to 1
		state = (x > 0) ? 0.5+state : 0.5-state ; //-ve or +ve direction
		bumpBase->bb->SetAnimation(animTX, state);

		state = fabs(y/Bump_BulletBase::BULLET_WORLD_SIZE);
		state *= 0.5; //reduce range to within  0 to 0.5 or 0.5 to 1
		state = (y > 0) ? 0.5+state : 0.5-state ; //-ve or +ve direction
		bumpBase->bb->SetAnimation(animTY, state);

		state = fabs(z/Bump_BulletBase::BULLET_WORLD_SIZE);
		state *= 0.5; //reduce range to within  0 to 0.5 or 0.5 to 1
		state = (z > 0) ? 0.5+state : 0.5-state ; //-ve or +ve direction
		bumpBase->bb->SetAnimation(animTZ, state);

		state = fabs((rz+PI)/(2*PI) );
				bumpBase->bb->SetAnimation(animRZ, state);

		state = fabs((-ry+PI)/(2*PI)); //Very important to negate RY
								bumpBase->bb->SetAnimation(animRY, state);


		state = fabs((rx+PI)/(2*PI));
		bumpBase->bb->SetAnimation(animRX, state);

		rposCurrent = _V(x, y, z);


		//Update terrain
		if(ground){

			//oapiEquToGlobal(base->hObjRefBody, lng, lat, base->rad, &gposSurfPoint);
			//oapiLocalToEqu(bumpBase->hObjRefBody, _V(m[12], m[13], m[14]), &lng, &lat, &rad);

			bumpBase->bb->Local2Global(rposCurrent, gposCurrent);
			oapiGlobalToEqu(bumpBase->hObjRefBody, gposCurrent, &lng, &lat, &rad);

			/*oapiWriteLogV("Bump_SimpleRigidBody::poststep: Converted (%f,%f,%f) to lolara(%f,%f,%f)",
					m[12], m[13], m[14], lng, lat, rad);
			*/

			int t = ground->updatePatch(lng, lat, rad);
		}

		testCollMsh.drawShape(transComponent);

	}//physics inited



	return 0;
}


int
Bump_SimpleRigidBody::clbkLoadState(FILEHANDLE scn)
{
	return 0;
}


int
Bump_SimpleRigidBody::clbkSaveState(FILEHANDLE scn)
{
	return 0;
}


/**
 *	Process keyboard input, this is not done for meshes currently.
 *
 *	TODO: Find a way to send commands to mesh based objects(say to set OpenGL camera focus
 *	or say a mesh based robot inside the base)
 */
int
Bump_SimpleRigidBody::processCommand()
{
/*	if(bbv->validKeyInput){

		//Command
		switch (bbv->keyInput.key)
		{
			case OAPI_KEY_UP:

				break;

			case OAPI_KEY_SPACE:
				break;



			case NO_KEY:

				break;

			default:
				break;
		}
	}
*/
	return 0;
}


/**
 * Creates the animation handles with the rotations around the axes as child anims
 * of the translations.
 *
 * It uses Bump_BulletBase::BULLET_WORLD_SIZE as the maximum size of the BulletBase VESSEL
 * Reads meshes from ROOT/Meshes/Bump/ComponentLibrary/
 *
 */
int
Bump_SimpleRigidBody::createOrbiterPrimitives()
{
	//Load the mesh
	VECTOR3 ofs = _V(0, 0, 0);
	//mshIndex = bumpBase->bb->AddMesh("Bump/ComponentLibrary/Sphere", &ofs);
	mshIndex = bumpBase->bb->AddMesh("Bump/ComponentLibrary/Plate", &ofs);


	//Animation setup
	static UINT group[] = {GROUP_INDEX}; // participating groups

	static MGROUP_TRANSLATE mgtTX( mshIndex, group, 0, _V(2*Bump_BulletBase::BULLET_WORLD_SIZE, 0, 0));
	animTX = bumpBase->bb->CreateAnimation (0.5);
	parent = bumpBase->bb->AddAnimationComponent (animTX, 0, 1, &mgtTX);

	mgtTY = new MGROUP_TRANSLATE( mshIndex, group, 0, _V(0, 2*Bump_BulletBase::BULLET_WORLD_SIZE, 0));
	animTY = bumpBase->bb->CreateAnimation (0.5);
	parent = bumpBase->bb->AddAnimationComponent (animTY, 0, 1, mgtTY, parent);

	mgtTZ = new MGROUP_TRANSLATE( mshIndex, group, 0, _V(0, 0, 2*Bump_BulletBase::BULLET_WORLD_SIZE));
	animTZ = bumpBase->bb->CreateAnimation (0.5);
	parent = bumpBase->bb->AddAnimationComponent (animTZ, 0, 1, mgtTZ, parent);

	mgrRZ = new MGROUP_ROTATE( mshIndex, group, 0, _V(0, 0, 0), _V(0, 0, 1), (float)(2 * PI) );
	animRZ = bumpBase->bb->CreateAnimation (0.0);
	parent = bumpBase->bb->AddAnimationComponent (animRZ, 0, 1, mgrRZ, parent);

	mgrRY = new MGROUP_ROTATE( mshIndex, group, 0, _V(0, 0, 0), _V(0, 1, 0), (float)(2 * PI) );
	animRY = bumpBase->bb->CreateAnimation (0.0);
	parent = bumpBase->bb->AddAnimationComponent (animRY, 0, 1, mgrRY, parent);

	mgrRX = new MGROUP_ROTATE( mshIndex, group, 1, _V(0, 0, 0), _V(1, 0, 0), (float)(2 * PI) );
	animRX = bumpBase->bb->CreateAnimation (0.0);
	parent = bumpBase->bb->AddAnimationComponent (animRX, 0, 1, mgrRX, parent);


	//Test equ to rel conversion

/*	double i = 0.f, limit = bumpBase->lng + (0.10* btScalar(PI)/180.f);
	double incr = (0.005* btScalar(PI)/180.f);
	VECTOR3 gposBall, gposBase;
	UINT mi;

	//Get reference body(nearest celestial body) co-ordinates for BB rpos calc.
	oapiGetGlobalPos(bumpBase->hObjBase, &gposBase);


	for(i=bumpBase->lng; i<limit; i+=incr){
		oapiEquToGlobal(oapiGetGbodyByName("Moon"), i, bumpBase->lat, bumpBase->rad, &gposBall);
		bumpBase->bb->Global2Local(gposBall , ofs);

		//ofs = gposBall - gposBase;

		mi = bumpBase->bb->AddMesh("Bump/ComponentLibrary/Sphere", &ofs);

		sprintf(boutput, "Bump_SimpleRigidBody::createOrbiterPrimitives: Created mesh %d at %f, %f, %f",
				mi, ofs.x, ofs.y, ofs.z);
		oapiWriteLog(boutput);
	}

*/



	return 0;
}


/**
 * Delete mesh and animation handles.
 *
 */
int
Bump_SimpleRigidBody::destroyOrbiterPrimitives()
{
	bool rv;

	rv = bumpBase->bb->DelMesh(mshIndex);
	if(rv){
		sprintf(boutput, "Bump_SimpleRigidBody::destroyOrbiterPrimitives: ERROR : Destroying Orbiter mesh(%d) failed for %s",
				mshIndex, strObjectName);
		oapiWriteLog(boutput);
		return 1;
	}

	if(ground)
		delete ground;


	return 0;
}


/**
 * Create the physics counterpart in the Bullet world.
 * localCreateRigidBody() does not add motion state, so make body directly
 */
int
Bump_SimpleRigidBody::createBulletRigidBody()
{

	btTransform trans;
	btQuaternion q;
	btVector3 ballInertia(0,0,0);


	//The ball
	//btCollisionShape* sphshpBall = new btSphereShape(1.f);
	btCollisionShape* shpSimpleBody = new btBoxShape(btVector3(2.5f, 0.05f, 2.5f));
	bumpBase->m_collisionShapes->push_back(shpSimpleBody);

	trans.setIdentity();
	trans.setOrigin(btVector3(count*20 + 100.f, 0.f + 1.f, 0.f));
	q.setEuler(0.f, 0.f, 0.f);
	trans.setRotation(q);
	mass = 0.5f;

	rbSimple = phyEngine->addRigidBodyMS(mass, trans, shpSimpleBody);
	//rbSimple = phyEngine->addKinematicBody(0.0f, trans, shpSimpleBody);

/*	rbSimple->setGravity(btVector3((btScalar)bumpBase->G.x,
											   (btScalar)bumpBase->G.y,
											   (btScalar)bumpBase->G.z));*/


	// Set up the test collision mesh
	testCollMsh.addShape(bumpBase, shpSimpleBody);


	return 0;
}


/**
 * Called by dtor and exit(). Do any physics related memory deallocation here
 */
int
Bump_SimpleRigidBody::destroyBulletRigidBody()
{
	physicsInited = false;

	delete ground;

	return 0;
}
